#include "StdAfx.h"
#include "helpers.h"
#include "jugadores.h"

double GoTo(Robot* robot, double x, double y, double velocidad)
{
	double dVelMax = velocidad;

	double dAngAGirar = CalcularAnguloAGirar(
						robot->pos.x, robot->pos.y,
                        x, y, robot->rotation);

    double t = ((cos(dAngAGirar) * cos(dAngAGirar)) * sign(cos(dAngAGirar)));
    double r = ((sin(dAngAGirar) * sin(dAngAGirar)) * sign(sin(dAngAGirar)));

    double dVelIzq = dVelMax * (t - r);
    double dVelDer = dVelMax * (t + r);

    double dif = (dVelIzq - dVelDer);

    double d = fabs(dif / (dVelMax * 2.0));
    double f = (1 - d) + 0.11;

	double potencia = 1;

    if ((dVelIzq * f) < 0) {
        robot->velocityLeft = dVelDer * f * potencia;
        robot->velocityRight = dVelIzq * f * potencia;
    }
    else {
        robot->velocityLeft = dVelIzq * f * potencia;
        robot->velocityRight = dVelDer * f * potencia;
    }

	return Distancia(robot->pos.x, robot->pos.y, x, y);
}

double GoTo_alternativo(Robot* robot, double x, double y, double velocidad)
{
	double vl, vr;
	double ang = robot->rotation;
	double targetAng = ObtenerAngulo(robot->pos.x, robot->pos.y, x, y);
	double deltaAng = Normalize180(targetAng - ang);

	double dist = Distancia(robot->pos.x, robot->pos.y, x, y);

	double modDelta = fabs(deltaAng);
	double k;

	if (modDelta > 100) {
        k = 0.50*velocidad;
    }
    else if (modDelta > 90) {
        k = 0.35*velocidad;
    }
    else if (modDelta > 50) {
        k = 0.20*velocidad;
    }
    else if (modDelta > 20) {
        k = 0.15*velocidad;
    }
    else if (modDelta >= 1) {
        k = 0.1*velocidad;
    }
    else {
        k = 0;
    }

	if(modDelta > 0) {
		vl = velocidad*(1.0 - exp(-dist/20));
        vr = (velocidad - 2*k)*(1.0 - exp(-dist/20));
	}
	else {
        vl = (velocidad - 2*k)*(1.0 - exp(-dist/20));
        vr = velocidad*(1.0 - exp(-dist/20));
	}

	robot->velocityLeft = vl;
	robot->velocityRight = vr;

	return dist;
}


double GoTo_viejo(Robot* robot, double x, double y, double velocidad)
{
	double vl, vr;
	double r = Normalize180(robot->rotation);
	double ang = ObtenerAngulo_viejo(robot->pos.x, robot->pos.y, x, y);
	double angleDiff = Normalize180(ang - r);

	if (fabs(angleDiff) < 15)
	{
		// Estoy apuntando mas o menos hacia la pelota => voy hacia adelante
		vl = velocidad;
		vr = velocidad;
	}
	else
	{
		// Ajusto orientacion
		vl = -angleDiff * ATENUAR;
		vr = angleDiff * ATENUAR;
	}

	robot->velocityLeft = vl;
	robot->velocityRight = vr;

	return Distancia(robot->pos.x, robot->pos.y, x, y);
}
